*=$0801
; 2018 SYS 2062
        !word   main-2, 10
        !byte   $9e
        !text   " 2062"
        !byte   $00,$00,$00

!set    MAXSPRITES = $20
!set    INDEX = $20        ;$CD00
!set    XPOS = $40         ; $CD80
!set    YPOS = $60         ; $CE00
!set    XMSB = $80         ; $CE80
!set    COLOR = $a0        ; $CF00
!set    POINTER = $c0      ; $CF80

!set    SPRITECOUNT =$fb   ; Number of sprites
!set    CURRENTSPRITE =$fc     ;Current sprite
!set    SWAPPED = $fd           ; For sort

;Zero page bit masks
!set    ANDBITS =$e0
!set    ANDBITS_NEG =$e8

!set    RASTERSETUP = 0

!set    DEBUG = 1

;Based on SQUARE DEMO disassembly. All logic intact.
;Did find some optimizations. Take that, 17-year old me!
;Pre-calculating the shifted sine table would save time
main
        ;clear screen
        LDA     #147
        JSR     $FFD2

        SEI
        LDA     #%01111111      ; switch off interrupt signals from CIA-1
        STA     $DC0D
        AND     $D011           ; clear most significant bit of VIC's raster register
        STA     $D011

        LDA     $DC0D           ; acknowledge pending interrupts from CIA-1
        LDA     $DD0D           ; acknowledge pending interrupts from CIA-2

        ;sprite data
        LDX     #63
SPRD    LDA     SPRIBITS,X
        STA     $3FC0,X
        DEX
        BPL     SPRD

;set up bit masks
        LDX     #7
        LDA     #128
NB      STA     ANDBITS,X
        PHA
        EOR     #$FF
        STA     ANDBITS_NEG,X
        PLA
        CLC
        ROR
        DEX
        BPL     NB

        JSR     SETUPMP

        JSR     TESTSPRITESSIN
        JSR     SORT

        LDA     #<TOPRASTER
        STA     $0314
        LDA     #>TOPRASTER
        STA     $0315
        LDA     #RASTERSETUP
        STA     $D012
        LDA     #1
        STA     $D01A
        CLI
        LDA     #0
        WAIT
        BEQ     WAIT


TOPRASTER
        LDA     $D019
        STA     $D019
!IFDEF  DEBUG{
        LDA     #8
        STA     $D020
        }
        JSR     UPDATESIN
!IFDEF  DEBUG{
        LDA     #2
        STA     $D020
        }
        JSR     SORT
!IFDEF  DEBUG{
        LDA     #0
        STA     $D020
        }

        LDA     #0
        STA     $D010
        ;We assume at least 8 sprites
        LDA     #255
        STA     $D015

; Setup first sprites
        LDY     #$00
LC09E   LDX     INDEX,Y
        LDA     COLOR,X
        STA     $D027,Y
        LDA     POINTER,X
        STA     $07F8,Y
        LDA     XMSB,X
        BEQ     LC0C4
        LDA     $d010
        ORA     ANDBITS,Y
        STA     $d010
LC0C4   TYA
        ASL
        TAY
        LDA     XPOS ,X
        STA     $D000,Y
        LDA     YPOS,X
        STA     $D001,Y
        TYA
        LSR
        TAY
        INY
        CPY     SPRITECOUNT
        BEQ     LC105
        CPY     #$08
        BNE     LC09E
        STY     CURRENTSPRITE


;Set up interrupt
        CLC
        LDX     INDEX
        LDA     YPOS,X
        ADC     #$15
;LC0EC  CMP #$F8
;LC0EE  BCS LC105 ; Check if sprite is below border, never mind
        STA     $D012

        LDA     #<nextsprite
        STA     $0314
        LDA     #>nextsprite
        STA     $0315


LC105   ; no more sprites

        !ifdef  DEBUG{
        LDA     #14
        STA     $D020
        }

        JMP     $EA81

;Individual sprite setup
nextsprite
    !ifdef  DEBUG{
        INC     $D020
        }
        LDA     $D019
        STA     $D019
        LDY     CURRENTSPRITE   ;Current sprite to show
NSLOOP  LDX     INDEX,Y
        TYA                 
        AND     #$07        ;Mask to find relevant hardware sprite
        TAY
        LDA     POINTER,X   ;Sprite pointer
        STA     $07F8,Y
        LDA     COLOR,X     ;Sprite color
        STA     $D027,Y
        LDA     XMSB,X      ;Sprite X high bit
        BEQ     NSNOH
        LDA     $D010
        ORA     ANDBITS,Y   ;Set X high bit
        BNE     NSNOH2
NSNOH   LDA     $D010
        AND     ANDBITS_NEG,Y    ;Clear X high bit
NSNOH2  STA     $D010
        TYA
        ASL
        TAY
        LDA     YPOS,X      ;Y position
        STA     $D001,Y
        LDA     XPOS,X      ;X Position
        STA     $D000,Y
        INC     CURRENTSPRITE   ;Go to next sprite
        LDY     CURRENTSPRITE
        CPY     SPRITECOUNT
        BEQ     NOMORESPRITES ;Stop if we are the end of the sprite list

        LDX     INDEX-8,y   ;Look at the topmost sprite currently being displayed
        LDA     YPOS,x
        ADC     #$15        ;Line at end of that sprite
        CMP     $D012       ; Is this the line we are already on or passed, then do it right away
        BEQ     NSLOOP
        BCC     NSLOOP       
        STA     $D012       ;If not, set next raster interrupt line
        !ifdef  DEBUG{
        DEC     $D020
        }
        JMP     $EA81

NOMORESPRITES
        LDA     #<TOPRASTER
        STA     $0314
        LDA     #>TOPRASTER
        STA     $0315
        LDA     #RASTERSETUP
        STA     $D012

        !ifdef  DEBUG{
        DEC     $D020
        }
        JMP     $EA81

TESTSPRITESSIN
        LDX     #0
TSS1
        LDA     #255
        STA     POINTER,X
        LDA     COLORS,X
        STA     COLOR,X
        INX
        CPX     #32
        BNE     TSS1
        STX     SPRITECOUNT
        JSR     UPDATESIN
        RTS

UPDATESIN
        INC     SINPOS
        LDX     #0
        LDY     SINPOS
USP1
        LDA     COSTABLE,Y
        CLC
        ADC     #35
        STA     XPOS,X
        LDA     #0
        ROL
        STA     XMSB,X
        LDA     SINTABLE,Y
        STA     YPOS,X
        TYA
        ;clc ; Not necessary, as xpos is never > 256+128 and ROL therefore never sets carry
        ADC     #8              ; JUMP 8 POSITIONS
        TAY
        INX
        CPX     SPRITECOUNT
        BNE     USP1
        RTS



SETUPMP
        LDX     #$00
LC009   LDA     #$00
        STA     XPOS,X
        STA     XMSB,X
        STA     COLOR,X
        STA     POINTER,X
        LDA     #$FF
        STA     YPOS,X
        TXA
        STA     INDEX,X
        INX
        CPX     #MAXSPRITES
        BNE     LC009
        RTS


SORT    ;why make it more complicated?
        ;mostly marginally faster than regular bubble sort, worst case is better
        LDX     #$01
LC073   LDY     INDEX,X
        LDA     YPOS,Y
        LDY     INDEX-1,X
        CMP     YPOS,Y
        BCS     LC096
        LDA     INDEX,X
        STA     INDEX-1,X
        TYA
        STA     INDEX,X
        ;If we have swapped and are >0 move back to see if it needs to go further up
        DEX
        BNE     LC073
        INX
LC096   INX
        CPX     SPRITECOUNT
        BNE     LC073
        RTS



SPRIBITS
        !byte   0,127,0,1,255,192,3,255,224,3,231,224
        !byte   7,217,240,7,223,240,7,217,240,3,231,224
        !byte   3,255,224,3,255,224,2,255,160,1,127,64
        !byte   1,62,64,0,156,128,0,156,128,0,73,0,0,73,0
        !byte   0,62,0,0,62,0,0,62,0,0,28,0

SINPOS 
        !byte   0

COSTABLE
        !fill  256, [137 + 110 * math.sin( i * 360 / 256 )]

SINTABLE
        !fill  256, [137 +80 * math.cos( i * 360 / 256 )]
    
COLORS
        !byte   0,1,2,3,4,5,7,8,9,10,11,12,13,14,15,0,1,2,3,4,5,7,8,9,10,11,12,13,14,15,1,2,3,4
